/*
 * manager.cpp
 *
 *  Created on: 6 mai 2012
 *      Author: user
 */


#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/subscriber.h>
#include <image_transport/image_transport.h>

#include <cv_bridge/CvBridge.h>
#include <opencv2/highgui/highgui.hpp>
//#include "window_thread.h"

#include <boost/thread.hpp>
#include <boost/format.hpp>

#ifdef HAVE_GTK
#include <gtk/gtk.h>

// Platform-specific workaround for #3026: image_view doesn't close when
// closing image window. On platforms using GTK+ we connect this to the
// window's "destroy" event so that image_view exits.
static void destroyNode(GtkWidget *widget, gpointer data)
{
  /// @todo On ros::shutdown(), the node hangs. Why?
  //ros::shutdown();
  exit(0); // brute force solution
}

static void destroyNodelet(GtkWidget *widget, gpointer data)
{
  // We can't actually unload the nodelet from here, but we can at least
  // unsubscribe from the image topic.
  reinterpret_cast<image_transport::Subscriber*>(data)->shutdown();
}
#endif

void imageCb(const sensor_msgs::ImageConstPtr& msg);

static const char WINDOW_NAME[] = "Kinect View";

image_transport::Subscriber sub_;
sensor_msgs::CvBridge img_bridge_;

boost::mutex image_mutex_;
sensor_msgs::ImageConstPtr last_msg_;
cv::Mat last_image_;

std::string window_name_= WINDOW_NAME;
boost::format filename_format_;
int count_;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "manager", ros::init_options::AnonymousName);
  //ROS_WARN("##Je demarre");

  //ros::NodeHandle nh =ros::this_node;
  ros::NodeHandle n;
  ros::NodeHandle nh("~");

  // Command line argument parsing
  // First positional argument is the transport type
  std::string transport = "raw";
  for (int i = 0; i < argc; ++i)
  {
    if (argv[i][0] != '-')
    {
      transport = argv[i];
      break;
    }
  }
  // Internal option, should be used only by the image_view node
  //bool shutdown_on_close = std::find(argv.begin(), argv.end(),
                                     //"--shutdown-on-close") != argv.end();

  /*
  std::string format_string;
  local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
  filename_format_.parse(format_string);

  //cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this);
//*/
  cv::namedWindow(window_name_);
 

  // Start the OpenCV window thread so we don't have to waitKey() somewhere
  //startWindowThread();


  image_transport::ImageTransport it(nh);
  //sub_ = it.subscribe(topic, 1, &imageCb, this, transport);
  sub_ = it.subscribe("/image", 3, &imageCb);

  //ROS_WARN("####spin");
  ros::spin();
  return 0;
}


//////////////////////////////////////////////////////////////////////////


void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
  //ROS_WARN("####Encoding:");
  //ROS_WARN(msg->encoding.c_str());
  image_mutex_.lock();

  // May want to view raw bayer data, which CvBridge doesn't know about
  if (msg->encoding.find("bayer") != std::string::npos)
  {
    last_image_ = cv::Mat(msg->height, msg->width, CV_8UC1,
                          const_cast<uint8_t*>(&msg->data[0]), msg->step);
  }
  // We want to scale floating point images so that they display nicely
  else if(msg->encoding.find("F") != std::string::npos)
  {

	  //ROS_WARN("####Floating:");
	  /*
	  	  cv::Mat  bridge;
	      try {
	        bridge = img_bridge_.imgMsgToCv(msg, "passthrough");
	      }
	      catch (sensor_msgs::CvBridgeException& e)
	      {
	          ROS_ERROR("Failed to transform depth image.");
	          return;
	      }

	      double min_range_=0.5;
	      double max_range_=5.5;

	      // convert to something visible
	      cv::Mat img(bridge.rows, bridge.cols, CV_8UC1);
	      for(int i = 0; i < bridge.rows; i++)
	      {
	          float* Di = bridge.ptr<float>(i);
	          char* Ii = img.ptr<char>(i);
	          for(int j = 0; j < bridge.cols; j++)
	          {
	              Ii[j] = (char) (255*((Di[j]-min_range_)/(max_range_-min_range_)));
	          }
	      }
    last_image_ = img;
    //*/


	    cv::Mat float_image_bridge = img_bridge_.imgMsgToCv(msg, "32FC1");
	    cv::Mat_<float> float_image = float_image_bridge;
	    float max_val = 0;
	    for(int i = 0; i < float_image.rows; ++i)
	    {
	      for(int j = 0; j < float_image.cols; ++j)
	      {
	        max_val = std::max(max_val, float_image(i, j));
	      }
	    }

	    if(max_val > 0)
	    {
	      float_image /= max_val;
	    }
	    last_image_ = float_image;
  }
  else
  {
    // Convert to OpenCV native BGR color
    try {
      last_image_ = img_bridge_.imgMsgToCv(msg, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException& e) {
    	ROS_ERROR("Unable to convert '%s' image to bgr8",
                             msg->encoding.c_str());

    }
  }

  // last_image_ may point to data owned by last_msg_, so we hang onto it for
  // the sake of mouseCb.
  last_msg_ = msg;

  // Must release the mutex before calling cv::imshow, or can deadlock against
  // OpenCV's window mutex.
  image_mutex_.unlock();
  if (!last_image_.empty()){
    cv::imshow(window_name_, last_image_);
    cv::waitKey(1);
  }
}



